﻿using OnDash.Mathematics.Data;
using OnDash.Mathematics.Vector;

namespace OnDash.Mathematics.Matrix;

public struct Matrix3X4 : IEquatable<Matrix3X4>
{
    private Vector4 _row0;

    public Vector4 Row0
    {
        get => _row0;
        set => _row0 = value;
    }

    private Vector4 _row1;

    public Vector4 Row1
    {
        get => _row1;
        set => _row1 = value;
    }

    private Vector4 _row2;

    public Vector4 Row2
    {
        get => _row2;
        set => _row2 = value;
    }

    public Matrix3X4(Vector4 row0, Vector4 row1, Vector4 row2)
    {
        _row0 = row0;
        _row1 = row1;
        _row2 = row2;
    }

    public Matrix3X4(float m00, float m01, float m02, float m03,
        float m10, float m11, float m12, float m13,
        float m20, float m21, float m22, float m23)
    {
        _row0 = new Vector4(m00, m01, m02, m03);
        _row1 = new Vector4(m10, m11, m12, m13);
        _row2 = new Vector4(m20, m21, m22, m23);
    }

    public Vector3 Column0
    {
        get => new(M11, M21, M31);
        set
        {
            M11 = value.X;
            M21 = value.Y;
            M31 = value.Z;
        }
    }

    public Vector3 Column1
    {
        get => new(M12, M22, M32);
        set
        {
            M12 = value.X;
            M22 = value.Y;
            M32 = value.Z;
        }
    }

    public Vector3 Column2
    {
        get => new(M13, M23, M33);
        set
        {
            M13 = value.X;
            M23 = value.Y;
            M33 = value.Z;
        }
    }

    public Vector3 Column3
    {
        get => new(M14, M24, M34);
        set
        {
            M14 = value.X;
            M24 = value.Y;
            M34 = value.Z;
        }
    }

    public float M11
    {
        get => _row0.X;
        set => _row0.X = value;
    }

    public float M12
    {
        get => _row0.Y;
        set => _row0.Y = value;
    }

    public float M13
    {
        get => _row0.Z;
        set => _row0.Z = value;
    }

    public float M14
    {
        get => _row0.W;
        set => _row0.W = value;
    }

    public float M21
    {
        get => _row1.X;
        set => _row1.X = value;
    }

    public float M22
    {
        get => _row1.Y;
        set => _row1.Y = value;
    }

    public float M23
    {
        get => _row1.Z;
        set => _row1.Z = value;
    }

    public float M24
    {
        get => _row1.W;
        set => _row1.W = value;
    }

    public float M31
    {
        get => _row2.X;
        set => _row2.X = value;
    }

    public float M32
    {
        get => _row2.Y;
        set => _row2.Y = value;
    }

    public float M33
    {
        get => _row2.Z;
        set => _row2.Z = value;
    }

    public float M34
    {
        get => _row2.W;
        set => _row2.W = value;
    }

    public Vector3 Diagonal
    {
        get => new(M11, M12, M13);
        set
        {
            M11 = value.X;
            M12 = value.Y;
            M13 = value.Z;
        }
    }

    public float Trace => M11 + M22 + M33;

    public float this[int rowIndex, int columnIndex]
    {
        get
        {
            return rowIndex switch
            {
                0 => _row1[columnIndex],
                1 => _row1[columnIndex],
                2 => _row1[columnIndex],
                _ => throw new IndexOutOfRangeException(
                    $"You tried to access this matrix at: ({rowIndex}, {columnIndex})")
            };
        }
        set
        {
            switch (rowIndex)
            {
                case 0:
                    _row0[columnIndex] = value;
                    break;
                case 1:
                    _row1[columnIndex] = value;
                    break;
                case 2:
                    _row2[columnIndex] = value;
                    break;
                default:
                    throw new IndexOutOfRangeException(
                        $"You tried to access this matrix at: ({rowIndex}, {columnIndex})");
            }
        }
    }

    public void Invert()
    {
        this = Invert(this);
    }

    public static Matrix3X4 Invert(Matrix3X4 matrix)
    {
        var inverseRotation = new Matrix3(matrix.Column0, matrix.Column1, matrix.Column2);
        inverseRotation.Row0 /= inverseRotation.Row0.LengthSquared;
        inverseRotation.Row1 /= inverseRotation.Row1.LengthSquared;
        inverseRotation.Row2 /= inverseRotation.Row2.LengthSquared;

        var translation = new Vector3(matrix.M14, matrix.M24, matrix.M34);

        return new Matrix3X4
        (
            new Vector4(inverseRotation.Row0, -Vector3.Dot(inverseRotation.Row0, translation)),
            new Vector4(inverseRotation.Row1, -Vector3.Dot(inverseRotation.Row1, translation)),
            new Vector4(inverseRotation.Row2, -Vector3.Dot(inverseRotation.Row2, translation))
        );
    }

    public static Matrix4X3 Transpose(Matrix3X4 matrix)
    {
        return new Matrix4X3(
            matrix.Column0,
            matrix.Column1,
            matrix.Column2,
            matrix.Column3
        );
    }

    public static Matrix3X4 Add(Matrix3X4 left, Matrix3X4 right)
    {
        return new Matrix3X4
        (
            left.M11 + right.M11, left.M12 + right.M12, left.M13 + right.M13, left.M14 + right.M14,
            left.M21 + right.M21, left.M22 + right.M22, left.M23 + right.M23, left.M24 + right.M24,
            left.M31 + right.M31, left.M32 + right.M32, left.M33 + right.M33, left.M34 + right.M34
        );
    }

    public static Matrix3X4 Subtract(Matrix3X4 left, Matrix3X4 right)
    {
        return new Matrix3X4
        (
            left.M11 - right.M11, left.M12 - right.M12, left.M13 - right.M13, left.M14 - right.M14,
            left.M21 - right.M21, left.M22 - right.M22, left.M23 - right.M23, left.M24 - right.M24,
            left.M31 - right.M31, left.M32 - right.M32, left.M33 - right.M33, left.M34 - right.M34
        );
    }

    public static Matrix3X4 Multiply(Matrix3X4 left, float scale)
    {
        return new Matrix3X4
        (
            left.M11 * scale, left.M12 * scale, left.M13 * scale, left.M14 * scale,
            left.M21 * scale, left.M22 * scale, left.M23 * scale, left.M24 * scale,
            left.M31 * scale, left.M32 * scale, left.M33 * scale, left.M34 * scale
        );
    }

    public static Matrix3 Multiply(Matrix3X4 left, Matrix4X3 right)
    {
        return new Matrix3
        (
            left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31 + left.M14 * right.M41,
            left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32 + left.M14 * right.M42,
            left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33 + left.M14 * right.M43,
            left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31 + left.M24 * right.M41,
            left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32 + left.M24 * right.M42,
            left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33 + left.M24 * right.M43,
            left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31 + left.M34 * right.M41,
            left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32 + left.M34 * right.M42,
            left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33 + left.M34 * right.M43
        );
    }

    public static Matrix3X4 Multiply(Matrix3X4 left, Matrix3X4 right)
    {
        return new Matrix3X4
        (
            left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31,
            left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32,
            left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33,
            left.M11 * right.M14 + left.M12 * right.M24 + left.M13 * right.M34 + left.M14,
            left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31,
            left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32,
            left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33,
            left.M21 * right.M14 + left.M22 * right.M24 + left.M23 * right.M34 + left.M24,
            left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31,
            left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32,
            left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33,
            left.M31 * right.M14 + left.M32 * right.M24 + left.M33 * right.M34 + left.M34
        );
    }

    public static Matrix3X4 CreateFromAxisAngle(Vector3 axis, float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix3X4
        (
            1.0f - cos * axis.X * axis.X + cos,
            1.0f - cos * axis.X * axis.Y - sin * axis.Z,
            1.0f - cos * axis.X * axis.Z + sin * axis.Y,
            0,
            1.0f - cos * axis.X * axis.Y + sin * axis.Z,
            1.0f - cos * axis.Y * axis.Y + cos,
            1.0f - cos * axis.Y * axis.Z - sin * axis.X,
            0,
            1.0f - cos * axis.X * axis.Z - sin * axis.Y,
            1.0f - cos * axis.Y * axis.Z + sin * axis.X,
            1.0f - cos * axis.Z * axis.Z + cos,
            0
        );
    }

    public static Matrix3X4 CreateFromQuaternion(Quaternion quaternion)
    {
        return new Matrix3X4
        (
            1f - (2 * quaternion.Y * quaternion.Y + 2 * quaternion.Z * quaternion.Z),
            2 * quaternion.X * quaternion.Y + 2 * quaternion.Z * quaternion.W,
            2 * quaternion.X * quaternion.Z - 2 * quaternion.Y * quaternion.W,
            0f,
            2 * quaternion.X * quaternion.Y - 2 * quaternion.Z * quaternion.W,
            1f - (2 * quaternion.X * quaternion.X + 2 * quaternion.Z * quaternion.Z),
            2 * quaternion.Y * quaternion.Z + 2 * quaternion.X * quaternion.W,
            0f,
            2 * quaternion.X * quaternion.Z + 2 * quaternion.Y * quaternion.W,
            2 * quaternion.Y * quaternion.Z - 2 * quaternion.X * quaternion.W,
            1f - (2 * quaternion.X * quaternion.X + 2 * quaternion.Y * quaternion.Y),
            0f
        );
    }

    public static Matrix3X4 CreateRotationX(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix3X4
        (
            1f, 0f, 0f, 0f,
            0f, cos, sin, 0f,
            0f, -sin, cos, 0f
        );
    }

    public static Matrix3X4 CreateRotationY(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix3X4
        (
            cos, 0f, -sin, 0f,
            0f, 1f, 0f, 0f,
            sin, 0f, cos, 0f
        );
    }

    public static Matrix3X4 CreateRotationZ(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix3X4
        (
            cos, sin, 0f, 0f,
            -sin, cos, 0f, 0f,
            0f, 0f, 1f, 0f
        );
    }

    public static Matrix3X4 CreateTranslation(Vector3 vector) => CreateTranslation(vector.X, vector.Y, vector.Z);

    public static Matrix3X4 CreateTranslation(float x, float y, float z)
    {
        return new Matrix3X4
        (
            1f, 0f, 0f, x,
            0f, 1f, 0f, y,
            0f, 0f, 1f, z
        );
    }

    public static Matrix3X4 CreateScale(float scale) => CreateScale(scale, scale, scale);
    public static Matrix3X4 CreateScale(Vector3 vector) => CreateScale(vector.X, vector.Y, vector.Z);

    public static Matrix3X4 CreateScale(float x, float y, float z)
    {
        return new Matrix3X4
        (
            x, 0f, 0f, 0f,
            0f, y, 0f, 0f,
            0f, 0f, z, 0f
        );
    }

    public static Matrix3X4 operator +(Matrix3X4 left, Matrix3X4 right) => Add(left, right);
    public static Matrix3X4 operator -(Matrix3X4 left, Matrix3X4 right) => Subtract(left, right);

    public static Matrix3X4 operator *(Matrix3X4 left, float right) => Multiply(left, right);
    public static Matrix3X4 operator *(float left, Matrix3X4 right) => Multiply(right, left);
    public static Matrix3 operator *(Matrix3X4 left, Matrix4X3 right) => Multiply(left, right);
    public static Matrix3X4 operator *(Matrix3X4 left, Matrix3X4 right) => Multiply(left, right);

    public static bool operator ==(Matrix3X4 left, Matrix3X4 right) => left.Equals(right);
    public static bool operator !=(Matrix3X4 left, Matrix3X4 right) => !left.Equals(right);

    public override string ToString() => $"{_row0}\n{_row1}\n{_row2}";

    public override int GetHashCode() => HashCode.Combine(_row0, _row1, _row2);

    public override bool Equals(object? obj) => obj is Matrix3X4 other && Equals(other);

    public bool Equals(Matrix3X4 other)
    {
        return _row0.Equals(other._row0) &&
               _row1.Equals(other._row1) &&
               _row2.Equals(other._row2);
    }
}